#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using Patrol = chapt4_interfaces::srv::Patrol;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;


class TurtleControl : public rclcpp::Node
{
private:
    
    OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
    rclcpp::Service<Patrol>::SharedPtr patrol_service_;

    //定义一个发布者
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; //发布者的智能指针
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber_;  //订阅者的智能指针
    //todo  改造，这里的xy 要从service 里面 接收
    double target_x_{1.0};
    double target_y_{1.0};
    double k_{1.0};   //比例系数
    double max_speed_{3.0};  //最大速度 
 
public:
  explicit TurtleControl(const std::string& node_name) : Node(node_name)
  {
    //1.先声明变量，过程中要修改的变量
    this->declare_parameter("k", 1.0);
    this->declare_parameter("max_speed", 1.0);
    //2.声明完了，获取变量 放到 这个k_ 赋值给上面的 private k_（比例系数）
    this->get_parameter("k", k_);
    this->get_parameter("max_speed", max_speed_); 

    this->set_parameter(rclcpp::Parameter("k", 2.0));  //通过代码的方式改变参数值


    parameters_callback_handle_ = this->add_on_set_parameters_callback(   //这种是通过服务，客户端调用的方式 修改参数的
      [&](const std::vector<rclcpp::Parameter>& parameters)->
      SetParametersResult
      {
        SetParametersResult result;
        result.successful = true; //result.successful 这里就不是指针了，直接. 就行了。
        for(const auto & param:parameters) {
             
              RCLCPP_INFO(this->get_logger(), "Parameter %s has changed to %f",param.get_name().c_str(), param.as_double());

              if(param.get_name() == "k") {
                k_ = param.as_double();
              }
              else if(param.get_name() == "max_speed") {
                max_speed_ = param.as_double();
              }
        }
        return result;
      }
    );

    //create_service<Patrol> 也是个模板函数，参数是Patrol
    this->patrol_service_ = this->create_service<Patrol>(
      "patrol",  //service name,这里的servicename 就是 ros2 service list 中看到的 /
      [&](const Patrol::Request::SharedPtr request,Patrol::Response::SharedPtr response)->void{  //[&] 代表捕获外部变量，& 代表捕获所有外部变量
        if(request->target_x>0 && request->target_x<12.0f && request->target_y>0 && request->target_y<12.0f){
          this->target_x_ = request->target_x;
          this->target_y_ = request->target_y;
          response->result = Patrol::Response::SUCCESS;
        }else{
          response->result = Patrol::Response::FAIL;
        }

        //额外调用我们添加的回调函数，这样做到第一时间更新参数
      }
    );

    //this->create_publisher 是从 rclcpp中继承而来
    //""turtle1/cmd_vel"" 是同 ros2 topic list 中查询而来 ,,turtle1 是第一只小海龟
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
    subscriber_ = this->create_subscription<turtlesim::msg::Pose>(
      "turtle1/pose", 
      10, 
      std::bind(&TurtleControl::on_pose_received, 
      this, 
      //代表on_pose_received 这个回调函数 是有参数的。且只有一个参数
      std::placeholders::_1)
    );
  }
  

  void on_pose_received(const turtlesim::msg::Pose::SharedPtr pose){ //有参数的，参数：收到数据的共享指针
    //todo 1.获取当前的位置，可以通过 ros2 interface show turtlesim/msg/Pose 来查看Pose 消息的结构
    auto current_x = pose->x;
    auto current_y = pose->y;
    RCLCPP_INFO(this->get_logger(), "Current position: (%f, %f)", current_x, current_y);

    //2.计算当前海龟位置和目标位置之间的距离差和角度。勾股定理
    auto distance = std::sqrt(
        (target_x_ - current_x)*(target_x_ - current_x) +
        (target_y_ - current_y)*(target_y_ - current_y)        
    );
    //pose->theta 是车头和 正向x轴  的夹角
    auto angle = std::atan2(target_y_ - current_y, target_x_ - current_x) - pose->theta;

    //3.控制策略
    //new 一个Twist 对象
    auto msg = geometry_msgs::msg::Twist();
    if(distance>0.1){
      if(fabs(angle)>0.1){
        //如果角度大于 0.1度 先转完,只会先旋转不会 前进或者后退运动。
        msg.angular.z = fabs(angle);
      }else{
        //直线情况，那就走直线
        msg.linear.x = k_*distance;
      }
      
    }
    //限制最大速度
    if(msg.linear.x>max_speed_){
      msg.linear.x = max_speed_;
    }
    
    //4.发布消息
    publisher_->publish(msg);
  }
};

int main(int argc, char **argv){
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TurtleControl>("turtle_control");
  rclcpp::spin(node); //运行node 这个节点。
  rclcpp::shutdown();
  return 0;
}